#include "PhysicsLocale.h"
#include "PhysicsEngine.h"

#include "../../Common/Util/Logger.h"
#include "../../Common/Util/Singulator.h"

using namespace Tool;
using namespace Physics;

PhysicsLocale::PhysicsLocale(void)
{
	Physics::PhysicsEngine& physics = Singulator<Physics::PhysicsEngine>::content();
	ACE_Guard<ACE_Recursive_Thread_Mutex> guard(physics.m_mutex);
	
	m_scene = physics.m_PhysicsSDK->createScene(physics.m_sceneDesc);

	assert(m_scene != NULL);

	// Create the default material
	NxMaterial* defaultMaterial = m_scene->getMaterialFromIndex(0); 
	defaultMaterial->setRestitution(BOUNCINESS);
	defaultMaterial->setStaticFriction(0.75);
	defaultMaterial->setDynamicFriction(0.75);

}

PhysicsLocale::~PhysicsLocale(void)
{
	if (m_scene)
	{
		// Make sure to fetchResults() before shutting down
		while (!m_scene->fetchResults(NX_RIGID_BODY_FINISHED, false));			
	
		NxActor** acts = m_scene->getActors();
		for(unsigned int i = 0; i < m_scene->getNbActors() - 1; i++)
		{
			m_scene->releaseActor(*acts[i]);
		}

		Physics::PhysicsEngine& physics = Singulator<Physics::PhysicsEngine>::content();
		physics.m_PhysicsSDK->releaseScene(*m_scene);
	}
}

void PhysicsLocale::update(float p_time)
{
	if (m_scene)
	{
		ACE_Guard<ACE_Recursive_Thread_Mutex> guard(m_mutex);

		// Get results from m_Scene->simulate(deltaTime)
		//while (!m_scene->fetchResults(NX_RIGID_BODY_FINISHED, false));

	    m_scene->fetchResults(NX_RIGID_BODY_FINISHED, false);

		// Start collision and dynamics for steptime since the last frame
		m_scene->simulate(p_time);
		m_scene->flushStream();
	
		NxActor** acts = m_scene->getActors();
		for(unsigned int i = 0; i < m_scene->getNbActors() - 1; i++)
		{
			if(acts[i]->userData == (void*)-1)
				m_scene->releaseActor(*acts[i]);
		}
	}
}

void PhysicsLocale::addObject(long long p_objectid, NxVec3 &p_position)
{
	try
	{
		ACE_Guard<ACE_Recursive_Thread_Mutex> guard(m_mutex);
		
		Physics::PhysicsEngine& physics = Singulator<Physics::PhysicsEngine>::content();
		
		PhysicsObject* physicsobject = physics.m_physicsObjects[p_objectid].get();


		// Add a single-shape actor to the scene
		NxSphereShapeDesc shapeDesc;
		shapeDesc.radius = float(SPHERE_RADIUS);
		physicsobject->m_actor = CreateActorFromDesc(shapeDesc, NxMat34(NxMat33(NX_IDENTITY_MATRIX), p_position), true);

		if (physicsobject->m_actor == NULL)
		{
			LOGDEBUG("ERROR CREATING AGEIA ACTOR!\n");

		}

		LOGDEBUG("Created Ageia object");
	}

	catch(Exception& e)
	{
		if (EX_FATAL == e) ABORT_APP;
	}
}

void PhysicsLocale::removeObject(long long p_objectid)
{
ACE_Guard<ACE_Recursive_Thread_Mutex> guard(m_mutex);





}

NxActor* PhysicsLocale::CreateActorFromDesc(const NxShapeDesc& shapeDesc, const NxMat34& globalPose, bool body)
{
	if (!m_scene) 
		return NULL;
	NxBodyDesc bodyDesc;
	NxActorDesc actorDesc;
	actorDesc.globalPose.t = globalPose.t;
	actorDesc.globalPose.M = globalPose.M;

	actorDesc.shapes.pushBack(&(NxShapeDesc&)shapeDesc);
	
	if (body)
	{
		bodyDesc.angularDamping	= 0.5f;
//			bodyDesc.flags |= NxBodyFlag::NX_BF_VISUALIZATION; 
		
		actorDesc.body			= &bodyDesc;
		actorDesc.density		= 10.0f;
		// enable visualization
	}
	if (!actorDesc.isValid())
	{
		LOGDEBUG("INVALID ACTOR SPEC!!\n");
	}

	NxActor* actor = NULL;
	//while (actor == NULL)
		actor = m_scene->createActor(actorDesc);
	
	if (actor == NULL)
	{	LOGDEBUG("Ageia ERROR: createActor returned NULL!");}

	return actor;
}

